/********************************************************************
*  Sang Phuoc Mai - maiphuocsang@gmail.com
*  Dec - 08 - 2014
*  Feetech Smart Servo Control
*
*  Version 1.0
*
*  File name: FeetechSCServo.cpp
*
*  References:
*   1. Dynamixel library: https://code.google.com/p/slide-33/downloads/list
*   2. Feetech page: http://www.rcservozone.com/Wiki.jsp?page=About%20SCServo
*
*    
*********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include "FeetechSCServo.h"
#include "Arduino.h"
#include "wiring_private.h"
#include "HardwareSerial.h"

unsigned char Instruction_Packet_Array[15]; // Array to hold instruction packet data 
unsigned char Status_Packet_Array[8];       // Array to hold returned status packet data
unsigned char Time_Counter;                 // Timer for time out watchers
unsigned char Status_Return_Value = READ;   // Status packet return states ( NON , READ , ALL )


//********************************************************************
//***********************Package format*******************************
//********************************************************************
//
//
//
//
//
//

void FeetechClass::transmitInstructionPacket(void) {
    unsigned char Counter;
    Counter = 0;

    //TODO: add reprocessor as references code so that the code can handle
    // more gereral cases
    Serial.write(HEADER);
    Serial.write(HEADER);
    Serial.write(Instruction_Packet_Array[0]);
    Serial.write(Instruction_Packet_Array[1]);

    do {
        Serial.write(Instruction_Packet_Array[Counter + 2]);
        Counter++;
    } while ((Instruction_Packet_Array[1] - 2) >= Counter);

    Serial.write(Instruction_Packet_Array[Counter + 2]);

    if((UCSR0A & B01100000) != B01100000) {
    	Serial.flush();
    }

}

unsigned int FeetechClass::readStatusPacket(void) {
    unsigned char Counter = 0x00;
    unsigned char First_Header = 0x00;

    Status_Packet_Array[0] = 0x00;
	Status_Packet_Array[1] = 0x00;
	Status_Packet_Array[2] = 0x00;
	Status_Packet_Array[3] = 0x00;
	

	Time_Counter = STATUS_PACKET_TIMEOUT + millis(); 									// Setup time out error

while(STATUS_FRAME_BUFFER >= Serial.available()){										// Wait for " header + header + frame length + error " RX data

	    if (millis() >= Time_Counter){
		return Status_Packet_Array[2] = B10000000;										// Return with Error if Serial data not received with in time limit
		}
} 
	
	if (Serial.peek() == 0xFF && First_Header != 0xFF){
		First_Header = Serial.read();													// Clear 1st header from RX buffer
		}else if (Serial.peek() == -1){
		return Status_Packet_Array[2] = B10000000;										// Return with Error if two headers are not found
		}
		if(Serial.peek() == 0xFF && First_Header == 0xFF){
			Serial.read();																// Clear 2nd header from RX buffer
			Status_Packet_Array[0] = Serial.read();                 					// ID sent from Dynamixel
			Status_Packet_Array[1] = Serial.read();										// Frame Length of status packet
			Status_Packet_Array[2] = Serial.read();										// Error byte 
			
			Time_Counter = STATUS_PACKET_TIMEOUT + millis();
				while(Status_Packet_Array[1] - 2 >= Serial.available()){				// Wait for wait for "Para1 + ... Para X" received data

					if (millis() >= Time_Counter){
					return Status_Packet_Array[2] = B10000000;							// Return with Error if Serial data not received with in time limit
					}
				} 
		
					do{
						Status_Packet_Array[3 + Counter] = Serial.read();
						Counter++;				
					}while(Status_Packet_Array[1] > Counter);							// Read Parameter(s) into array
			
			Status_Packet_Array[Counter + 4] = Serial.read();							// Read Check sum	
				
		}else{
		return Status_Packet_Array[2] = B10000000;										// Return with Error if two headers are not found
		}
}

unsigned int FeetechClass::settingResult(unsigned char servoID) {

}

unsigned int FeetechClass::readingResult(unsigned char servoID) {

}

unsigned int FeetechClass::servoPing(unsigned char servoID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = PING_LENGTH;
	Instruction_Packet_Array[2] = CMD_PING;
	Instruction_Packet_Array[3] = (~(ServoID + PING_LENGTH + CMD_PING) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();
	readStatusPacket();

	if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
		return (Status_Packet_Array[0]);			// Return SERVO ID
	}else{
		return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
	} 
}

unsigned int FeetechClass::enableTorque(unsigned char servoID, bool isEnable) {
	
	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = ENABLE_TORQUE_LENGTH; // length = 4
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE;
	Instruction_Packet_Array[4] = isEnable;
	Instruction_Packet_Array[5] = (~(ID + ENABLE_TORQUE_LENGTH + CMD_WRITE_DATA + RAM_TORQUE_ENABLE + isEnable)) && 0xFF;

    while (Serial.read() != -1); //clear RX buffer

    transmitInstructionPacket();

    //TODO next
    if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}
}

unsigned int FeetechClass::servoAction(unsigned char servoID, unsigned int setPosition, unsigned int setSpeed) {
	
	byte setPosition_L = setPosition >> 8;
	byte setPosition_H = setPosition & 0xFF;
	byte setSpeed_L = setSpeed >> 8;
	byte setSpeed_H = setSpeed && 0xFF;

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SERVO_ACTION_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
	Instruction_Packet_Array[4] = setPosition_L; //Low bit first
	Instruction_Packet_Array[5] = setPosition_H; //High bit
	Instruction_Packet_Array[6] = setSpeed_L; //low bit first
	Instruction_Packet_Array[7] = setSpeed_H; //then high bit
	Instruction_Packet_Array[8] = (~(servoID + SERVO_ACTION_LENGTH + CMD_WRITE_DATA + RAM_GOAL_POSITION_L + setPosition_L 
		                             + setPosition_H + setSpeed_L + setSpeed_H) && 0xFF);

	while (Serial.read()) != -1);		//clear RX buffer

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}

}

unsigned int FeetechClass::setDefaultPosition(unsigned char servoID, unsigned int setPosition, unsigned int setSpeed) {

	byte setPosition_L = setPosition >> 8;
	byte setPosition_H = setPosition & 0xFF;
	byte setSpeed_L = setSpeed >> 8;
	byte setSpeed_H = setSpeed && 0xFF;

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SERVO_WRITE_DEFAULT_LENGTH;
	Instruction_Packet_Array[2] = CMD_REG_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
	Instruction_Packet_Array[4] = setPosition_L; //Low bit first
	Instruction_Packet_Array[5] = setPosition_H; //High bit
	Instruction_Packet_Array[6] = setSpeed_L; //low bit first
	Instruction_Packet_Array[7] = setSpeed_H; //then high bit
	Instruction_Packet_Array[8] = (~(servoID + SERVO_ACTION_LENGTH + CMD_WRITE_DATA + RAM_GOAL_POSITION_L + setPosition_L 
		                             + setPosition_H + setSpeed_L + setSpeed_H) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();	
	
	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}

}

//TODO double check below function because it's diffences from orginal firmware ()
// the original firmware did not use specific ID but use broadcast ID ?? why is it?
// can it only reset specific servo??? need to check
unsigned int FeetechClass::resetDefaultPosition(unsigned char servoID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = RESET_LENGTH;
	Instruction_Packet_Array[2] = COMMAND_ACTION;
	Instruction_Packet_Array[3] = (~(servoID + ACTION_LENGTH + COMMAND_ACTION) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}		
}

// to be implemented FeetechClass::wheelSync = SyncWritePos later because I do not know what is it

unsigned int FeetechClass::setID(unsigned char servoID, unsigned char new_ID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_ID_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_ID;
	Instruction_Packet_Array[4] = new_ID;
	Instruction_Packet_Array[5] = (~(servoID + SET_ID_LENGTH + CMND_WRITE_DATA+ EEPROM_ID + new_ID) && 0xFF); 

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}
}

// TODO need to implement setStatusPaket because it'll update Status_Return_Value
// but what is that?
unsigned int FeetechClass::setModeAndLimitAngle(unsigned char servoID, bool servoMode, unsigned int minAngle, unsigned int maxAngle) {

	byte minAngle_L = minAngle >> 8;
	byte minAngle_H = minAngle & 0xFF;
	byte maxAngle_L = maxAngle >> 8;
	byte maxAngle_H = maxAngle & 0xFF;

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_MODE_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_MIN_ANGLE_LIMIT_L;

	if (servoMode == WHEEL) {
		Instruction_Packet_Array[4] = 0x00;
		Instruction_Packet_Array[5] = 0x00;
		Instruction_Packet_Array[6] = 0x00;
		Instruction_Packet_Array[7] = 0x00;
		Instruction_Packet_Array[8] = (~(ID + SET_MODE_LENGTH + CMD_WRITE_DATA + EEPROM_MIN_ANGLE_LIMIT_L) && 0xFF);
	} else {
		Instruction_Packet_Array[4] = minAngle_L;
		Instruction_Packet_Array[5] = minAngle_H;
		Instruction_Packet_Array[6] = maxAngle_L;
		Instruction_Packet_Array[7] = maxAngle_H;
		Instruction_Packet_Array[8] = (~(ID + SET_MODE_LENGTH + CMD_WRITE_DATA + EEPROM_MIN_ANGLE_LIMIT_L + minAngle_L + minAngle_H + maxAngle_L + maxAngle_H) && 0xFF);
	}

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (Status_Return_Value == ALL){
		readStatusPacket();
		if (Status_Packet_Array[2] != 0){
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}		
	}
}

unsigned int FeetechClass::setLimitTorque(unsigned char servoID, int maxTorque) {

	byte maxTorque_L = maxTorque >> 8;
	byte maxTorque_H = maxTorque && 0xFF;

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ;
	Instruction_Packet_Array[4] = maxTorque_L;
	Instruction_Packet_Array[5] = maxTorque_H;
	Instruction_Packet_Array[6] = (~(servoID + SET_MAX_TORQUE_LENGTH + CMD_WRITE_DATA + EEPROM_MAX_TORQUE_L + maxTorque_L + maxTorque_H) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}
}

unsigned int FeetechClass::setPunch(unsigned char servoID, unsigned int punch) {
    
	byte punch_L = punch >> 8;
	byte punch_H = punch & 0xFF;

    Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_PUNCH_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_PUNCH_L;
	Instruction_Packet_Array[4] = punch_L;
	Instruction_Packet_Array[5] = punch_H;
	Instruction_Packet_Array[6] = (~(servoID + SET_PUNCH_LENGTH + CMD_WRITE_DATA + RAM_PUNCH_L + punch_L + punch_H) && 0xFF );

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}	
}
//TODO need to double check this one
unsigned int FeetechClass::setBaudRate(unsigned char servoID, long newBaudRate) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_BD_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_BAUD_RATE;
	
	switch (newBaudRate) {
		case 1000000:
			Instruction_Packet_Array[4] = B_1M;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_1M ) & 0xFF); 
			break;
		case 500000:
			Instruction_Packet_Array[4] = B_0_5M;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_0_5M ) & 0xFF); 
			break;
		case 250000:
			Instruction_Packet_Array[4] = B_250K;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_250K ) & 0xFF); 
			break;
		case 128000:
			Instruction_Packet_Array[4] = B_128K;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_128K ) & 0xFF); 
			break;
		case 115200:
			Instruction_Packet_Array[4] = B_115200;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_115200 ) & 0xFF); 
			break;
		case 76800:
			Instruction_Packet_Array[4] = B_76800;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_76800 ) & 0xFF); 
			break;
		case 57600:
			Instruction_Packet_Array[4] = B_57600;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_57600 ) & 0xFF); 
			break;
		case 38400:
			Instruction_Packet_Array[4] = B_38400;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE +B_38400 ) & 0xFF); 
			break;
		default:
			Instruction_Packet_Array[4] = B_1M;
			Instruction_Packet_Array[5] = (~(servoID + SET_BD_LENGTH + CMD_WRITE_DATA + EEPROM_BAUD_RATE + B_1M ) & 0xFF); 
	}
	
	while(Serial.read() != -1);

	transmitInstructionPacket()

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}
}

unsigned int FeetechClass::setComplianceMargin(unsigned char servoID, unsigned int CCW, unsigned int CW) {
	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_COMP_MARGIN;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_CW_COMPLIANCE_MARGIN;
	Instruction_Packet_Array[4] = CW;
	Instruction_Packet_Array[5] = CCW;
	Instruction_Packet_Array[6] = (~(servoID + SET_COMP_MARGIN + CMD_WRITE_DATA + RAM_CW_COMPLIANCE_MARGIN + CW + CCW) && 0xFF );

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}

}

unsigned int FeetechClass::lockEprom(unsigned char servoID, bool isLock) {

	int lockStatus = LOCK; //lock as default

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = LOCK_ROM_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_ROM_LOCK;

	if (isLock) {
		lockStatus = LOCK;
	} else {
		lockStatus = UNLOCK; //unlock
	}

	Instruction_Packet_Array[4] = lockStatus;
	Instruction_Packet_Array[5] = (~(servoID + LOCK_ROM_LENGTH + CMD_WRITE_DATA + EEPROM_ROM_LOCK + lockStatus) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}
}

unsigned int FeetechClass::setPID(unsigned char servoID, unsigned char P, unsigned char I, unsigned char D) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_PID_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN;
	Instruction_Packet_Array[4] = P;
	Instruction_Packet_Array[5] = I;
	Instruction_Packet_Array[6] = D;
	Instruction_Packet_Array[7] = (~(servoID + SET_PID_LENGTH + CMD_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}	

}

unsigned int FeetechClass::setSpeed(unsigned char servoID, unsigned int newSpeed) {

	//TODO do we really need below function, need to be consider
	unsigned int goalSpeed = newSpeed;

	if (newSpeed < 0)
	{
		goalSpeed = -newSpeed;
		goalSpeed |= (1 << 10);
	}

	byte goalSpeed_L = goalSpeed >> 8;
	byte goalSpeed_H = goalSpeed & 0xFF;

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = SET_SPEED_LENGTH;
	Instruction_Packet_Array[2] = CMD_WRITE_DATA;
	Instruction_Packet_Array[3] = EEPROM_GOAL_SPEED_L;
	Instruction_Packet_Array[4] = goalSpeed_L;
	Instruction_Packet_Array[5] = goalSpeed_H;
	Instruction_Packet_Array[6] = (~(servoID + SET_SPEED_LENGTH + CMD_WRITE_DATA + EEPROM_GOAL_SPEED_L + goalSpeed_L + goalSpeed_H) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();

	if (servoID == 0XFE || Status_Return_Value != ALL ){		// If ID of FE is used no status packets are returned so we do not need to check it
		return (0x00);
	}else{
		readStatusPacket();
		if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
			return (Status_Packet_Array[0]);			// Return SERVO ID
		}else{
			return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
		}
	}	
}

unsigned int FeetechClass::setLimitTemperature(unsigned char servoID, unsigned char maxTemperature){

}

unsigned int FeetechClass::setLimitVoltage(unsigned char servoID, unsigned int maxVolteage){

}
unsigned int FeetechClass::readPosition(unsigned char servoID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = READ_POS_LENGTH;
	Instruction_Packet_Array[2] = CMD_READ_DATA;
	Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
	Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
	Instruction_Packet_Array[5] = (~(servoID + READ_POS_LENGTH + CMD_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();
	readStatusPacket();
	
	if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value											// If there is no status packet error return value
		return Status_Packet_Array[4] << 8 | Status_Packet_Array[3];	// Return present position value
	}else{
		return (Status_Packet_Array[2] | 0xF000);            				// If there is a error Returns error value
	}
}

unsigned int FeetechClass::readVoltage(unsigned char servoID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = READ_VOLT_LENGTH;
	Instruction_Packet_Array[2] = CMD_READ_DATA;
	Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE;
	Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
	Instruction_Packet_Array[5] = (~(servoID + READ_VOLT_LENGTH + CMD_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();
	readStatusPacket();
	
	if (Status_Packet_Array[2] == 0){ 					// If there is no status packet error return value
		return Status_Packet_Array[3];					// Return voltage value (value retured by Dynamixel is 10 times actual voltage)
	}else{
		return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
	}	

}

unsigned int FeetechClass::readTemperature(unsigned char servoID) {

	Instruction_Packet_Array[0] = servoID;
	Instruction_Packet_Array[1] = READ_TEMP_LENGTH;
	Instruction_Packet_Array[2] = CMD_READ_DATA;
	Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE;
	Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
	Instruction_Packet_Array[5] = (~(servoID + READ_TEMP_LENGTH + CMD_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH) && 0xFF);

	while (Serial.read() != -1);

	transmitInstructionPacket();
	readStatusPacket();	

	if (Status_Packet_Array[2] == 0){ 				// If there is no status packet error return value
		return Status_Packet_Array[3];
	}else{
		return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
	}
}

unsigned int FeetechClass::readLoad(unsigned char servoID){

}

unsigned int FeetechClass::readSpeed(unsigned char servoID){

}

unsigned int FeetechClass::readLockStatus(unsigned char servoID) {

}

FeetechClass SCServo;